﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Team6_Algorithms.Filters;
using System.Drawing;
using System.Drawing.Imaging;

namespace Team6_Algorithms.HighEnd
{
    public class CannyEdge
    {
        int LowerThreshold = 30;    // Gradient strength nescessary to start edge
        int UpperTreshold = 60;     // Minimum gradient strength to continue edge
        int GX;                     // Sum of Sobel mask products values in the x direction
        int GY;                     // Sum of Sobel mask products values in the y direction

        public int[,] EdgeDirections;
        public float[,] Strength;

        int nWidth;
        int nHeight;
        int stride;
        
        unsafe byte* gScan0;

        public Bitmap Run(Bitmap c)
        {
            Masks M = new Masks();
            Mask GxMask = M.SobelHori;
            Mask GyMask = M.SobelVerti;

            int pointerShift;

            # region Gaussian and Direction filtering
            
            Bitmap B = (Bitmap)c.Clone();

            //Apply Gaussian Blur Mask 5x5 size to the image
            Masking.applyFilter(B, M.Gaussian5x5);

            Bitmap bSrc = (Bitmap)B.Clone();
            BitmapData BmData = B.LockBits(new Rectangle(0, 0, B.Width, B.Height), ImageLockMode.ReadWrite, PixelFormat.Format24bppRgb);
            BitmapData BmSrc = bSrc.LockBits(new Rectangle(0, 0, bSrc.Width, bSrc.Height), ImageLockMode.ReadWrite, PixelFormat.Format24bppRgb);
            stride = BmData.Stride;
            nWidth = B.Width;
            nHeight = B.Height;
            Strength = new float[nWidth, nHeight];
            EdgeDirections = new int[nWidth, nHeight];

            System.IntPtr Scan0 = BmData.Scan0;
            System.IntPtr SrcScan0 = BmSrc.Scan0;

            //Find the directions and strenghts of the edges
            unsafe
            {
                gScan0 = (byte*)(void*)Scan0;
                byte* p = (byte*)(void*)Scan0;
                byte* pSrc = (byte*)(void*)SrcScan0;

                int x = 0, y = 0;
                for (y = 1; y < nHeight - 1; y++)
                {
                    for (x = 1; x < nWidth - 1; x++)
                    {
                        pointerShift = (y * stride) + (x * 3);
                        byte* temp = pSrc + pointerShift;
                        calculateMasks(temp, GxMask, GyMask, stride);

                        //Calculate strenght of the gradient
                        Strength[x, y] = (float)Math.Sqrt(Math.Pow(GX, 2) + Math.Pow(GY, 2));
                        
                        //Calculate edge direction
                        double calculatedEdgeDirection = (Math.Atan2(GX, GY) / Math.PI) * 180;
                        
                        int edgeDirection = 0;

                        //Devide the edge values in 4 groups
                        if (((calculatedEdgeDirection < 22.5) && (calculatedEdgeDirection > -22.5))|| (calculatedEdgeDirection > 157.5) || (calculatedEdgeDirection < -157.5))
                            edgeDirection = 0;
                        if (((calculatedEdgeDirection > 22.5) && (calculatedEdgeDirection < 67.5)) || ((calculatedEdgeDirection < -112.5) && (calculatedEdgeDirection > -157.5)))
                            edgeDirection = 45;
                        if (((calculatedEdgeDirection > 67.5) && (calculatedEdgeDirection < 112.5)) || ((calculatedEdgeDirection < -67.5) && (calculatedEdgeDirection > -112.5)))
                            edgeDirection = 90;
                        if (((calculatedEdgeDirection > 112.5) && (calculatedEdgeDirection < 157.5)) || ((calculatedEdgeDirection < -22.5) && (calculatedEdgeDirection > -67.5)))
                            edgeDirection = 135;

                        //Put the new Angle values of each pixel in an array
                        EdgeDirections[x, y] = edgeDirection;

                    }
                }
            # endregion



            # region Walking trough the edges

                for (y = 1; y < nHeight - 1; y++)
                {
                    for (x = 1; x < nWidth - 1; x++)
                    {
                        if (Strength[x, y] > UpperTreshold)
                        {
                            switch (EdgeDirections[x, y])
                            {
                                case 0:
                                    findEdge(0, 1, y, x, 0, LowerThreshold);
                                    break;
                                case 45:
                                    findEdge(1, 1, y, x, 45, LowerThreshold);
                                    break;
                                case 90:
                                    findEdge(1, 0, y, x, 90, LowerThreshold);
                                    break;
                                case 135:
                                    findEdge(1, -1, y, x, 135, LowerThreshold);
                                    break;
                                default:
                                    pointerShift = (y * stride) + (x * 3);

                                    gScan0[pointerShift] = (byte)0;
                                    gScan0[pointerShift + 1] = (byte)0;
                                    gScan0[pointerShift + 2] = (byte)0;
                                    break;
                            }
                        }
                        else
                        {
                            pointerShift = (y * stride) + (x * 3);
                            gScan0[pointerShift] = (byte)0;
                            gScan0[pointerShift + 1] = (byte)0;
                            gScan0[pointerShift + 2] = (byte)0;
                        }
                    }
                }

                for (y = 0; y < nHeight; y++)
                {
                    for (x = 0; x < nWidth; x++)
                    {
                        pointerShift = (y * stride) + (y * 3);
                        if ((gScan0[pointerShift] != 255 && gScan0[pointerShift] != 0) ||
                            (gScan0[pointerShift + 1] != 255 && gScan0[pointerShift + 1] != 0) ||
                            (gScan0[pointerShift + 2] != 255 && gScan0[pointerShift + 2] != 0))
                        {
                            gScan0[pointerShift] = (byte)0;
                            gScan0[pointerShift + 1] = (byte)0;
                            gScan0[pointerShift + 2] = (byte)0;
                        }
                    }
                }

            #endregion



            #region Suppressing nonMax values

                for (y = 1; y < nHeight - 1; y++)
                {
                    for (x = 1; x < nWidth - 1; x++)
                    {
                        pointerShift = (y * stride) + (y * 3);
                        if (gScan0[pointerShift] == 255)
                        {
                            switch (EdgeDirections[x, y])
                            {
                                case 0:
                                    suppressNonMax(0, 1, y, x, 0, LowerThreshold);
                                    break;
                                case 45:
                                    suppressNonMax(1, 1, y, x, 45, LowerThreshold);
                                    break;
                                case 90:
                                    suppressNonMax(1, 0, y, x, 90, LowerThreshold);
                                    break;
                                case 135:
                                    suppressNonMax(1, -1, y, x, 135, LowerThreshold);
                                    break;
                                default:
                                    break;
                            }
                        }
                    }

            #endregion

                }
            }

            B.UnlockBits(BmData);
            bSrc.UnlockBits(BmSrc);
            return B;
        }
      


        private  unsafe void  calculateMasks(byte* P, Mask XMASK, Mask YMASK,  int STRIDE)
        {
            GX = 0;
            GY = 0;
            int yCounter = 0;
            int xCounter = 0;
            byte* CalcPointer = P;
            int Boundary = (XMASK.Size - 1) / 2;
            for (int y = -Boundary; y <= Boundary; ++y)
            {

                for (int x = -Boundary; x <= Boundary; ++x)
                {
                    GX += CalcPointer[2 + (x * 3) + (y * STRIDE)] * XMASK.MaskValues[xCounter][yCounter];
                    GY += CalcPointer[2 + (x * 3) + (y * STRIDE)] * YMASK.MaskValues[xCounter][yCounter];

                    xCounter++;

                }
                xCounter = 0;
                yCounter++;
            }
           
            //if value is too high or too low as a result of the mask calculation, set it to 0 (in case of too low) or to 255 (in case of too high)
            if (GX > 255)
                GX = 255;            

            if (GX < 0)
                GX = 0;
            
            if (GY > 255)
                GY = 255;
           
            if (GY < 0)
                GY = 0;

        }

        void findEdge(int rowShift, int collumnShift, int row, int collumn, int direction, int lowerThreshold)
        {
            int newRow = 0, newCollumn = 0;
            bool edgeEnd = false;
            int pointerShift;
            
            if (collumnShift < 0)
            {
                if (collumn > 0)
                    newCollumn = collumn + collumnShift;
                else
                    edgeEnd = true;
            }
            else if (collumn < nWidth - 1)
            {
                newCollumn = collumn + collumnShift;
            }
            else
                edgeEnd = true;		// If the next pixel would be off image, don't do the while loop
            
            if (rowShift < 0)
            {
                if (row > 0)
                    newRow = row + rowShift;
                else
                    edgeEnd = true;
            }
            else if (row < nHeight - 1)
            {
                newRow = row + rowShift;
            }
            else
                edgeEnd = true;
            
            unsafe
            {

                while (EdgeDirections[newCollumn, newRow] == direction && !edgeEnd && Strength[newCollumn, newRow] > lowerThreshold)
                {
                    pointerShift = (newRow * stride) + (newCollumn * 3);
                    gScan0[pointerShift] =  (byte)255;
                    gScan0[pointerShift + 1] = (byte)255;
                    gScan0[pointerShift + 2] = (byte)255;


                    if (collumnShift < 0)
                    {
                        if (newCollumn > 0)
                            newCollumn = newCollumn + collumnShift;
                        else
                            edgeEnd = true;
                    }
                    else if (newCollumn < nWidth - 1)
                    {
                        newCollumn = newCollumn + collumnShift;
                    }
                    else
                        edgeEnd = true;
                    if (rowShift < 0)
                    {
                        if (newRow > 0)
                            newRow = newRow + rowShift;
                        else
                            edgeEnd = true;
                    }
                    else if (newRow < nHeight - 1)
                    {
                        newRow = newRow + rowShift;
                    }
                    else
                        edgeEnd = true;	

                }
            }

        }

        void suppressNonMax(int rowShift, int colShift, int row, int collumn, int direction, int lowerThreshold)
        {
            int newRow = 0, newCol = 0;
            bool edgeEnd = false;
            int pointerShift;
            int pixelCount = 0;		
            int count;
            float[,] nonMax = new float[nWidth, 3];
            int[] max = new int[3];



            if (colShift < 0)
            {
                if (collumn > 0)
                    newCol = collumn + colShift;
                else
                    edgeEnd = true;
            }
            else if (collumn < nWidth - 1)
            {
                newCol = collumn + colShift;
            }
            else
                edgeEnd = true;		// If the next pixel would be off image, don't do the while loop
            if (rowShift < 0)
            {
                if (row > 0)
                    newRow = row + rowShift;
                else
                    edgeEnd = true;
            }
            else if (row < nHeight - 1)
            {
                newRow = row + rowShift;
            }
            else
                edgeEnd = true;
            unsafe
            {
                pointerShift = (newRow * stride) + (newCol * 3);
                while ((EdgeDirections[newCol, newRow] == direction) && !edgeEnd && (gScan0[pointerShift] == 255))
                {
                    if (colShift < 0)
                    {
                        if (newCol > 0)
                            newCol = newCol + colShift;
                        else
                            edgeEnd = true;
                    }
                    else if (newCol < nWidth - 1)
                    {
                        newCol = newCol + colShift;
                    }
                    else
                        edgeEnd = true;
                    if (rowShift < 0)
                    {
                        if (newRow > 0)
                            newRow = newRow + rowShift;
                        else
                            edgeEnd = true;
                    }
                    else if (newRow < nHeight - 1)
                    {
                        newRow = newRow + rowShift;
                    }
                    else
                        edgeEnd = true;

                    nonMax[pixelCount, 0] = newRow;
                    nonMax[pixelCount, 1] = newCol;
                    nonMax[pixelCount, 2] = Strength[newCol, newRow];
                    pixelCount++;
                    pointerShift = (newRow * stride) + (newCol * 3);


                }

                edgeEnd = false;
                colShift *= -1;
                rowShift *= -1;
                if (colShift < 0)
                {
                    if (collumn > 0)
                        newCol = collumn + colShift;
                    else
                        edgeEnd = true;
                }
                else if (collumn < nWidth - 1)
                {
                    newCol = collumn + colShift;
                }
                else
                    edgeEnd = true;
                if (rowShift < 0)
                {
                    if (row > 0)
                        newRow = row + rowShift;
                    else
                        edgeEnd = true;
                }
                else if (row < nHeight - 1)
                {
                    newRow = row + rowShift;
                }
                else
                    edgeEnd = true;

                pointerShift = (newRow * stride) + (newCol * 3);
                while ((EdgeDirections[newCol, newRow] == direction) && !edgeEnd && (gScan0[pointerShift] == 255))
                {
                    if (colShift < 0)
                    {
                        if (newCol > 0)
                            newCol = newCol + colShift;
                        else
                            edgeEnd = true;
                    }
                    else if (newCol < nWidth - 1)
                    {
                        newCol = newCol + colShift;
                    }
                    else
                        edgeEnd = true;
                    if (rowShift < 0)
                    {
                        if (newRow > 0)
                            newRow = newRow + rowShift;
                        else
                            edgeEnd = true;
                    }
                    else if (newRow < nHeight - 1)
                    {
                        newRow = newRow + rowShift;
                    }
                    else
                        edgeEnd = true;

                    nonMax[pixelCount, 0] = newRow;
                    nonMax[pixelCount, 1] = newCol;
                    nonMax[pixelCount, 2] = Strength[newCol, newRow];
                    pixelCount++;
                    pointerShift = (newRow * stride) + (newCol * 3);
                    
                    /* Suppress non-maximum edges */
                    max[0] = 0;
                    max[1] = 0;
                    max[2] = 0;
                    for (count = 0; count < pixelCount; count++)
                    {
                        if (nonMax[count, 2] > max[2])
                        {
                            max[0] = (int)nonMax[count, 0];
                            max[1] = (int)nonMax[count, 1];
                            max[2] = (int)nonMax[count, 2];
                        }
                    }
                    for (count = 0; count < pixelCount; count++)
                    {
                        pointerShift = (((int)nonMax[count, 0]) * stride) + (((int)nonMax[count, 1]) * 3);
                        gScan0[pointerShift] = 0;
                        gScan0[pointerShift + 1] = 0;
                        gScan0[pointerShift + 2] = 0;
                        
                    }
                    

                }

            }

        }

       public static unsafe void makeBlackNWhite(Bitmap B)
        {
            int height = B.Height;
            int width = B.Width;
            int x, y;
            int pointerShift;
            BitmapData BmData = B.LockBits(new Rectangle(0, 0, B.Width, B.Height), ImageLockMode.ReadWrite, PixelFormat.Format24bppRgb);
            int stride = BmData.Stride;
            System.IntPtr Scan0 = BmData.Scan0;
            byte* p = (byte*)(void*)Scan0;
            for (y = 0; y < height; y++)
            {
                for (x = 0; x < width; x++)
                {
                    pointerShift = (y * stride) + (x * 3);
                    Color D = Color.FromArgb(p[pointerShift + 2], p[pointerShift + 1], p[pointerShift]);
                    if (D != Color.White)
                    {

                        p[pointerShift] = (byte)0;
                        p[pointerShift + 1] = (byte)0;
                        p[pointerShift + 2] = (byte)0;
                        Console.WriteLine("Should be black");
                    }
                }
            }
            B.UnlockBits(BmData);

        }
    }
}
